﻿#include "semiautoplinesfinder.h"
#include "plinesfit_opencv.h"
#include "opencv2/highgui.hpp"
#include <QDebug>

using namespace cv;

int sign(int x)
{
    return x == 0 ? 0 : x / abs(x);
}
void drawPoints(cv::Mat & edge, std::vector<cv::Point2d> lines, cv::Vec3b color)
{
    for(int i = 0; i < lines.size(); i++)
    {
        cv::Point2d p = lines.at(i);
        //cv::circle(edge, p, 2, color, 1);
        edge.at<cv::Vec3b>(p.y, p.x) = color;
        qDebug() << p.x << ", " << p.y;
    }
}

/**
 * @brief semiautoPlinesFinder 在一幅图像中半自动的提取出两条平行线，并计算距离
 * @param edge  输入图像，这个图像是已经做边缘提取之后的了。
 * @param start  起点
 * @param end 终点，沿着起点->终点这条直线的方向找边缘，并且是在这条直线的附近范围（离直线距离不超过 BAR_LEN）
 * @param [out] p1  p1->p2 形成的直线与两条平行线垂直，并且 p1 在第一条平行线上，p2 在第二条平行线上。
 * @param [out] p2  p1->p2 的中点过 start->end 形成的直线。
 * @param BAR_LEN 在直线距离不超过 BAR_LEN 的范围内找数据点
 * @return
 */
double semiautoPlinesFinder(cv::Mat &debug, cv::Mat &edge, cv::Point2i start, cv::Point2i end,
                            cv::Point2d &p1, cv::Point2d &p2, int BAR_LEN )
{
    std::vector<cv::Point2d> line1, line2;
    findPline(edge, start ,end, line1, line2, BAR_LEN); // 到这里还是对的
//    qDebug() << "line1.size " << line1.size();
//    qDebug() << "line2.size " << line2.size();
//    drawPoints(debug, line1, cv::Vec3b(0, 255, 0));
//    drawPoints(debug, line2, cv::Vec3b(0, 0, 255));
    if(line1.size() < 2 || line2.size() < 2)
    {
        return false;
    }

    // 到这里表示已经把两条平行线的数据点提取出来了。
    double aa, bb, c1, c2, tau = 3;
    int N = 100;
    bool ret = weightedPLinesFit(line1, line2, aa, bb, c1, c2, tau, N); //到这里两条平行线的方程也计算出来了
    if(!ret)
    {
        return false;
    }
    cv::Point2d t1 = crossPoint(start, end, aa, bb, c1);// start->end 与第一条平行线的交点
    cv::Point2d t2 = crossPoint(start, end, aa, bb, c2);// start->end 与第二条平行线的交点
    cv::Point2d t_mid = midPoint(t1, t2); // 这个点到两条平行线的距离相等，并且过直线 start->end
    double dist = abs(c1 - c2);
    double a = bb, b = - aa;
    double c = -(t_mid.x * a + t_mid.y * b); // ax+by+c=0 与两条平行线垂直。并且过 t_mid

    p1 = crossPoint(a, b, c, aa, bb, c1); // 这条垂线与第一条平行线的交点是 p1
    p2 = crossPoint(a, b, c, aa, bb, c2); // 这条垂线与第二条平行线的交点是 p2

    qDebug() << p1.x << ", " << p1.y;
    qDebug() << p2.x << ", " << p2.y;

    qDebug() << abs(c1 - c1);

    return dist;
}

bool cagaus(double A[],double b[], int perm[], int N, double x[]);

/**
 * @brief crossPoint 计算两条直线的交点
 * @param line_eq1 三个元素的数组，存放 [a, b, c], 这里 a x + b y + c = 0 是第一条直线
 * @param line_eq2 三个元素的数组，存放 [a, b, c], 这里 a x + b y + c = 0 是第二条直线
 * @return 交点坐标，如果失败则返回 Point2d(-DBL_MAX, -DBL_MAX)
 */
cv::Point2d crossPoint(const double line_eq1[3], const double line_eq2[2])
{
    double A[4] = {line_eq1[0], line_eq1[1],
                   line_eq2[0], line_eq2[1]};
    double B[2] = {-line_eq1[2], -line_eq2[2]};
    double xy[2];
    int perm[2];
    bool suc = cagaus(A, B, perm, 2, xy);

    double x = xy[0];
    double y = xy[1];
    if( suc )
    {
        return cv::Point2d(x, y);
    }
    return cv::Point2d(-DBL_MAX, -DBL_MAX);
}

/**
 * @brief crossPoint 计算两条直线的交点
 * @param a1 第一条直线的系数， 这里 a1 x + b1 y + c1 = 0 是第一条直线
 * @param b1 第一条直线的系数， 这里 a1 x + b1 y + c1 = 0 是第一条直线
 * @param c1 第一条直线的系数， 这里 a1 x + b1 y + c1 = 0 是第一条直线
 * @param a2 第二条直线的系数， 这里 a2 x + b2 y + c2 = 0 是第一条直线
 * @param b2 第二条直线的系数， 这里 a2 x + b2 y + c2 = 0 是第一条直线
 * @param c2 第二条直线的系数， 这里 a2 x + b2 y + c2 = 0 是第一条直线
 * @return 交点坐标，如果失败则返回 Point2d(-DBL_MAX, -DBL_MAX)
 */
cv::Point2d crossPoint(double a1, double b1, double c1, double a2, double b2, double c2)
{
    double A[4] = {a1, b1, a2, b2};
    double B[2] = {-c1, -c2};
    double xy[2];
    int perm[2];

    bool suc = cagaus(A, B, perm, 2, xy);

    double x = xy[0];
    double y = xy[1];
    if( suc )
    {
        return cv::Point2d(x, y);
    }

    return cv::Point2d(-DBL_MAX, -DBL_MAX);

}

/**
 * @brief crossPoint 计算两条直线的交点
 * @param p1 第一条直线给出直线上两个点的坐标
 * @param p2 第一条直线给出直线上两个点的坐标
 * @param a 第二条直线的系数， 这里 a2 x + b2 y + c2 = 0 是第一条直线
 * @param b 第二条直线的系数， 这里 a2 x + b2 y + c2 = 0 是第一条直线
 * @param c 第二条直线的系数， 这里 a2 x + b2 y + c2 = 0 是第一条直线
 * @return
 */
cv::Point2d crossPoint(cv::Point2i p1, cv::Point2i p2, double a2, double b2, double c2)
{
    double length = hypot( p2.x - p1.x, p2.y - p1.y );
    double b1 = ( p2.x - p1.x ) / length;
    double a1 = -( p2.y - p1.y ) / length;
    double c1 = -(a1 * p1.x + b1 * p1.y);

//    qDebug() << "distance between p1 and a1 x + b1 y + c = 0 is " << a1 * p1.x + b1 * p1.y + c1;
//    qDebug() << "distance between p2 and a1 x + b1 y + c = 0 is " << a1 * p2.x + b1 * p2.y + c1;

    return crossPoint(a1, b1, c1, a2, b2, c2);
}


/**
 * @brief midPoint 计算两个点的中点
 * @param p1
 * @param p2
 * @return
 */
cv::Point2d midPoint(cv::Point2d p1, cv::Point2d p2)
{
    return cv::Point2d((p1.x + p2.x) / 2, (p1.y + p2.y) / 2);
}


/**
 * @brief findPline 在一幅图像中将两个平行线的坐标点提取出来。
 * @param edge 输入图像，这个图像是已经做边缘提取之后的了。
 * @param start 起点
 * @param end 终点，沿着起点->终点这条直线的方向找边缘，并且是在这条直线的附近范围（离直线距离不超过 BAR_LEN）
 * @param line1 找到的第一条平行线的数据点
 * @param line2 找到的第二条平行线的数据点
 * @param BAR_LEN 在直线距离不超过 BAR_LEN 的范围内找数据点
 */
void findPline(const cv::Mat &edge, cv::Point2i start, cv::Point2i end,
                 std::vector<cv::Point2d> &line1,
                 std::vector<cv::Point2d> &line2, int BAR_LEN)
{
    line1.clear();
    line2.clear();
    double length = hypot( end.x - start.x, end.y - start.y );
    double cos_theta0 = ( end.x - start.x ) / length;
    double sin_theta0 = ( end.y - start.y ) / length;

    double cos_theta = - sin_theta0; // 与主直线垂直
    double sin_theta = cos_theta0;
    for(int dist = -BAR_LEN; dist <= BAR_LEN; dist ++)
    {
        int x1 = static_cast<int>(start.x - dist * cos_theta);
        int y1 = static_cast<int>(start.y - dist * sin_theta);

        int x2 = static_cast<int>(end.x - dist * cos_theta);
        int y2 = static_cast<int>(end.y - dist * sin_theta);

        cv::Point2i s1(x1, y1), e1(x2, y2);
//        cv::line(mat, s1, e1, cv::Scalar(0, 0, 255));
        Point2d p = findCrossPoint(edge, s1, e1);
        if(p.x >= 0)
        {
            line1.push_back(p);
        }
        p = findCrossPoint(edge, e1, s1);
        if(p.x >= 0)
        {
            line2.push_back(p);
        }
    }
}


/**
 * @brief findCrossPoint 从 start 向 end 方向遍历，找到一个值为 255 的点。
 * @param edge 做完阈值处理的图像。在这个图像中沿直线方向寻找第一个 255 点。
 * @param start 直线段的起点。
 * @param end 直线段的终点，在这个范围内寻找值为 255 的点。
 * @return 返回找到的点。如果没找到任何点，则返回 Point2d(-1, -1)  图像上不会有负数坐标的点的。所以可以区分正常点。
 */
cv::Point2d findCrossPoint(const Mat &edge, cv::Point2i start, cv::Point2i end)
{
    int detX = end.x - start.x;
    int detY = end.y - start.y;
    int x_max = edge.cols;
    int y_max = edge.rows;
    if(abs(detX) > abs(detY))
    {
        int det = sign(detX);
        for(int x = start.x; x != end.x; x += det)
        {
            int y = (static_cast<double>(x - start.x) / detX) * detY + start.y;
            if(x < 0 || x > x_max || y < 0 || y > y_max) break;
            int z = edge.at<uchar> (Point2i(x, y));
            if(z == 255)
            {
                return Point2d(x, y);
            }
            z = edge.at<uchar> (Point2i(x, y + 1));
            if(z == 255)
            {
                return Point2d(x, y + 1);
            }
        }
    }
    else
    {
       // cv::imshow("", edge);
        int det = sign(detY);
        for(int y = start.y; y != end.y; y += det)
        {
            int x = (static_cast<double>(y - start.y) / detY) * detX + start.x;
            if(x < 0 || x > x_max || y < 0 || y > y_max) break;
            int z = edge.at<uchar> (Point2i(x, y));
            if(z == 255)
            {
                return Point2d(x, y);
            }
            z = edge.at<uchar> (Point2i(x + 1, y));
            if(z == 255)
            {
                return Point2d(x + 1, y);
            }
        }
    }
    return Point2d(-1, -1);
}
